package com.gitee.wsl.mathematics.complex.quaternion.ext

import com.gitee.wsl.mathematics.matrix.mat4.MutableMatrix4
import com.gitee.wsl.mathematics.complex.quaternion.Quaternion
import com.gitee.wsl.mathematics.matrix.mat4.Matrix4
import com.gitee.wsl.mathematics.number.compareTo


/** Converts a quaternion to a 4 * 4 matrix.    */
fun<T:Number> MutableMatrix4<T, *, *>.set(q: Quaternion<T, *>){

    val qxx = q.x * q.x
    val qyy = q.y * q.y
    val qzz = q.z * q.z
    val qxz = q.x * q.z
    val qxy = q.x * q.y
    val qyz = q.y * q.z
    val qwx = q.w * q.x
    val qwy = q.w * q.y
    val qwz = q.w * q.z

    this[0, 0] = 1f - 2f * (qyy + qzz)
    this[0, 1] = 2f * (qxy + qwz)
    this[0, 2] = 2f * (qxz - qwy)

    this[1, 0] = 2f * (qxy - qwz)
    this[1, 1] = 1f - 2f * (qxx + qzz)
    this[1, 2] = 2f * (qyz + qwx)

    this[2, 0] = 2f * (qxz + qwy)
    this[2, 1] = 2f * (qyz - qwx)
    this[2, 2] = 1f - 2f * (qxx + qyy)

}

fun<T:Number> Quaternion<T, *>.set(matrix:Matrix4<T, *, *>) = matrix.build(this)

/**
 * Construct a quaternion using a [Mat4]
 */
fun<T:Number> Matrix4<T, *, *>.build(q: Quaternion<T, *>) = with(q){
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
    // The code is a conversion from the `setFromAxis` method from LibGDX.
    val m = transposed
    val tr = m.trace
    //val tr = m.x.x + m.y.y + m.z.z

    if (tr > zero) {
        var s = sqrt(tr + 1f)
        val qw = s * 0.5f
        s = 0.5f / s
        val qx = (m.z.y - m.y.z) * s
        val qy = (m.x.z - m.z.x) * s
        val qz = (m.y.x - m.x.y) * s
        set(qx, qy, qz, qw)
    } else if ((m.x.x > m.y.y) && (m.x.x > m.z.z)) {
        var s = sqrt(1f + m.x.x - m.y.y - m.z.z)
        val qx = s * 0.5f
        s = 0.5f / s
        val qy = (m.y.x + m.x.y) * s
        val qz = (m.x.z + m.z.x) * s
        val qw = (m.z.y - m.y.z) * s
        set(qx, qy, qz, qw)
    } else if (m.y.y > m.z.z) {
        var s = sqrt(1.0f + m.y.y - m.x.x - m.z.z)
        val qy = s * 0.5f
        s = 0.5f / s
        val qx = (m.y.x + m.x.y) * s
        val qz = (m.y.z + m.z.y) * s
        val qw = (m.x.z - m.z.x) * s
        set(qx, qy, qz, qw)
    } else {
        var s = sqrt(1.0f + m.z.z - m.x.x - m.y.y)
        val qz = s * 0.5f
        s = 0.5f / s
        val qx = (m.x.z + m.z.x) * s
        val qy = (m.z.y + m.y.z) * s
        val qw = (m.y.x - m.x.y) * s
        set(qx, qy, qz, qw)
    }
}
